www.gusucode.com > ROS Deployed on Arduino running in External Mode in Simulink 程序工具箱matlab源码 > ROS Deployed on Arduino running in External Mode in Simulink/sfun_ros_interface.cpp

    /*
 * sfun_ros_interface.cpp: S-Function for Demonstrating ROS Deployed to
 * Arduino from within Simulink
 *
 * Using Template from sfuntmpl_basic from Mathworks and linked with code
 * from rosserial_arduino
 *
 * Author: Gavin Kane
 *         gavin.kane@hs-emden-leer.de
 *
 * License: BSD
 *
 *
 * Prepare for the deployment to arduino by calling 
 *     "mex sfun_ros_interface.cpp"
 * in the matlab command window
 */

#define S_FUNCTION_NAME  sfun_ros_interface
#define S_FUNCTION_LEVEL 2


/*
 * Need to include simstruc.h for the definition of the SimStruct and
 * its associated macro definitions.
 */
#include "simstruc.h"

/*
 * ROS and ARDUINO Defines and Includes
 */
uint16_T servoValue1 = 0;
boolean_T   outputEnable = false;

#define USE_STM32_HW_SERIAL
#define ARDUINO 100
/*
 * All ROS and Arduino Code must be kept inside the #ifndef lines
 */

#ifndef MATLAB_MEX_FILE
#include <ros.h>
//#include <std_msgs/String.h>
#include <std_msgs/Empty.h>
#include <std_msgs/UInt16.h>
#include <rosserial_arduino/Adc.h>

ros::NodeHandle  nh;

void servo_cb_1( const std_msgs::UInt16& cmd_msg){
    servoValue1 = cmd_msg.data;
}

void messageCb( const std_msgs::Empty& toggle_msg){
  digitalWrite(13, HIGH-digitalRead(13));   // blink the led
  outputEnable = !outputEnable;
}

ros::Subscriber<std_msgs::Empty> sub_led("toggle_led", messageCb );
ros::Subscriber<std_msgs::UInt16> sub_link_1("robot_link_1", servo_cb_1 );

rosserial_arduino::Adc adc_msg;
ros::Publisher adc_pub("adc", &adc_msg);

#endif

/*====================*
 * S-function methods *
 *====================*/

/* Function: mdlInitializeSizes ===============================================
 * Abstract:
 *    The sizes information is used by Simulink to determine the S-function
 *    block's characteristics (number of inputs, outputs, states, etc.).
 */
static void mdlInitializeSizes(SimStruct *S)
{
    ssSetNumSFcnParams(S, 0);  /* Number of expected parameters */
    if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) {
        /* Return if number of expected != number of actual parameters */
        return;
    }

    ssSetNumContStates(S, 0);
    ssSetNumDiscStates(S, 0);

    if (!ssSetNumInputPorts(S, 1)) return;
    ssSetInputPortWidth(S, 0, 6);
    ssSetInputPortDataType(S, 0, SS_UINT16);
    ssSetInputPortRequiredContiguous(S, 0, true); /*direct input signal access*/
    /*
     * Set direct feedthrough flag (1=yes, 0=no).
     * A port has direct feedthrough if the input is used in either
     * the mdlOutputs or mdlGetTimeOfNextVarHit functions.
     */

    if (!ssSetNumOutputPorts(S, 2)) return;
    ssSetOutputPortWidth(S, 0, 1);
    ssSetOutputPortDataType(S, 0, SS_UINT16);
    ssSetOutputPortWidth(S, 1, 1);
    ssSetOutputPortDataType(S, 1, SS_BOOLEAN);
    
    ssSetNumSampleTimes(S, 1);
    ssSetNumRWork(S, 0);
    ssSetNumIWork(S, 0);
    ssSetNumPWork(S, 0);
    ssSetNumModes(S, 0);
    ssSetNumNonsampledZCs(S, 0);

    /* Specify the sim state compliance to be same as a built-in block */
    ssSetSimStateCompliance(S, USE_DEFAULT_SIM_STATE);

    ssSetOptions(S, 0);

}



/* Function: mdlInitializeSampleTimes =========================================
 * Abstract:
 *    This function is used to specify the sample time(s) for your
 *    S-function. You must register the same number of sample times as
 *    specified in ssSetNumSampleTimes.
 */
static void mdlInitializeSampleTimes(SimStruct *S)
{
    ssSetSampleTime(S, 0, CONTINUOUS_SAMPLE_TIME);
    ssSetOffsetTime(S, 0, 0.0);
}


#define MDL_START  /* Change to #undef to remove function */
#if defined(MDL_START) 
  /* Function: mdlStart =======================================================
   * Abstract:
   *    This function is called once at start of model execution. 
   *
   *    In this section we start up the node and advertise and subscribe to
   *    topics as applicable
   */
  static void mdlStart(SimStruct *S)
  {
     #ifndef MATLAB_MEX_FILE
        pinMode(13, OUTPUT);
        nh.initNode();
        nh.advertise(adc_pub);
        nh.subscribe(sub_led);
        nh.subscribe(sub_link_1);
     #endif
  }
#endif /*  MDL_START */



/* Function: mdlOutputs =======================================================
 * Abstract:
 *    In this function, inputs are used for the data to be published, and
 *    data gained from previous callbacks are set to the outputs.
 */
static void mdlOutputs(SimStruct *S, int_T tid)
{
  
    const uint16_T *u = (const uint16_T*) ssGetInputPortSignal(S,0);
    uint16_T       *servo = (uint16_T*)ssGetOutputPortSignal(S,0);
    boolean_T      *led_enable = (boolean_T*)ssGetOutputPortSignal(S,1);
    
    #ifndef MATLAB_MEX_FILE
      adc_msg.adc0 = u[0];
      adc_msg.adc1 = u[1];
      adc_msg.adc2 = u[2];
      adc_msg.adc3 = u[3];
      adc_msg.adc4 = u[4];
      adc_msg.adc5 = u[5];

      adc_pub.publish(&adc_msg);
      nh.spinOnce();
    #endif

    servo[0] = servoValue1;
    led_enable[0] = outputEnable;
}


/* Function: mdlTerminate =====================================================
 * Abstract:
 *    In this function, you should perform any actions that are necessary
 *    at the termination of a simulation.  For example, if memory was
 *    allocated in mdlStart, this is the place to free it.
 */
static void mdlTerminate(SimStruct *S)
{
}


/*=============================*
 * Required S-function trailer *
 *=============================*/

#ifdef  MATLAB_MEX_FILE    /* Is this file being compiled as a MEX-file? */
#include "simulink.c"      /* MEX-file interface mechanism */
#else
#include "cg_sfun.h"       /* Code generation registration function */
#endif